#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
'''
@File         :time_varying_formation_controller.py
@Description  :
@Time         :2024/11/20 17:25:07
@Author       :Lin Yuheng
@Version      :1.0
'''



import os
import sys
# import pandas as pd
import numpy as np

sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), ".")))
# refer to: https://docs.python-guide.org/writing/structure/

import rospy
from formation_controller import FormationController
from tools.formation_structure import *
from tools.mathlib import *
from tools.vector import *
from tools.filter import *
from tools.increment_pid_controller import *
from tools.pid_controller import *
from tools.params import *
from autopilot.L1_Controller import L1Controller
from autopilot.TECS import TECSController

from mavros_msgs.msg import State


class TimeVaryingFormationController(FormationController):
    def __init__(self):
        super(TimeVaryingFormationController, self).__init__()

        # 本机在编队中的编号
        self.idx = 1

        # 控制器时间戳
        self.ctrl_timestamp = 0.0

        # 滤波后的领机（虚拟领机）信息
        self._leader_states_f = S_FwLeaderStates()
        # 滤波后的本机信息
        self._fw_state_f = S_FwStates()

        # 是否使用滤波器对于原始数据滤波
        self._use_the_filter = False

        # 检验计算本机的空速（状态）以及实际读取的空速的合法性
        self._fw_airspd_states_vaild = True
        # 检验计算领机的空速（状态）以及实际读取的空速的合法性
        self._led_airspd_states_vaild = True

        # 领机空速向量
        self._led_airsp = Vector()
        # 领机地速向量
        self._led_gspeed_2d = Vector()

        # 本机空速向量
        self._fw_airspd = Vector()
        # 本机地速向量
        self._fw_gspeed_2d = Vector()
        # 本机风估计向量
        self._fw_wind_vector = Vector()

        # 领机dir_cos，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._led_cos_dir = 0.0
        # 领机dir_sin，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._led_sin_dir = 0.0
        # 本机dir_cos，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._fw_cos_dir = 0.0
        # 本机dir_sin，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._fw_sin_dir = 0.0

        # 领机航向角
        self._led_yaw = 0.0
        self._led_yaw_prev = 0.0
        # 领机航向角速度
        self._led_yaw_dot = 0.0

        # 控制器X方向混合误差产生参数
        self._mix_Xerr_params = S_MixXerrParams()
        # 控制器Y方向混合误差产生参数
        self._mix_Yerr_params = S_MixYerrParams()

        # 计算产生期望地速的PID控制器
        self._gspeed_sp_pid = PIDController()

        # 重置内部控制器标志量
        self._rest_speed_pid = False

        # 从机期望地速增量
        self._del_fol_gspeed = 0.0

        # 飞机期望空速
        self._airspeed_sp = 0.0

        # 飞机期望空速（上一时刻）
        self._airspeed_sp_prev = 0.0

        # 本机飞机空速期望值滤波器
        self._airspeed_sp_filter = Filter()

        # （虚拟）领机glo速度x滤波器
        self._led_glo_vel_x_filter = Filter()
        # （虚拟）领机glo速度y滤波器
        self._led_glo_vel_y_filter = Filter()

        # （虚拟）领机偏航角yaw滤波器
        self._led_yaw_filter = Filter()
        # （虚拟）领机偏航角速度yaw_dot滤波器
        self._led_yaw_dot_filter = Filter()
        
        # 期望加速度a_t_sp滤波器
        self._a_t_sp_filter = Filter()
        # 期望滚转角phi_sp滤波器
        self._phi_sp_filter = Filter()

        """
        TECS函数,变量（组）
        """
        # TECS控制器
        self._tecs = TECSController()

        # 重置TECS控制器标志量
        self._rest_tecs = False

        # 纵向速度有效标志位
        self._vz_valid = False

        # TECS控制器参数
        self._tecs_params = TECSParams()

        """
        L1控制器函数,变量（组）
        """
        # 横侧向控制器
        self._l1_controller = L1Controller()

        # 横侧向控制器参数
        self._lateral_params = LateralControllerParams()

        # 产生期望滚转角的PID控制器
        self._roll_sp_pid = PIDController()

        # 重置横侧向控制器
        self._reset_lateral_controller = False

        # 最终roll通道控制量
        self._roll_cmd = 0.0

        # 最终roll通道控制量（上一时刻）
        self._roll_cmd_prev = 0.0

        # 期望滚转角滤波器
        self._roll_cmd_filter = Filter()

        # 横向与纵向通道混合误差
        self.mix_err_Xk = 0
        self.mix_err_Yk = 0

        """
        一致性编队函数，变量（组）
        """
        self.agent_num = 3

        # X-Y平面编队偏移量H 相对于编队中心坐标系, px py vx vy
        self.H = np.array(
            [[-10, 0, 0, 0],
             [-20, -5, 0, 0],
             [-20, 5, 0, 0],])

        self.H_rot = self.H
        self.H_rot_prev = self.H

        # # adjacency matrix with virtual leader connection
        # self.Adj_VL = np.array(
        #     [[0, 1, 1],
        #      [1, 0, 1],
        #      [1, 1, 0]
        #     ]
        # )

        # adjacency matrix without virtual leader connection
        self.Adj = np.array(
            [[0, 1, 1],
             [1, 0, 1],
             [1, 1, 0]
            ]
        )

        # 邻居节点状态
        self._neighbor_pos = np.zeros((self.agent_num, 2))
        self._neighbor_vel = np.zeros((self.agent_num, 2))

        # 打印数据头标志位
        self.data_head = False

    def set_formation_offset(self):
        """
        @Description :设置编队偏移量
        @Param:
        @Returns     :
        """
        self.formation_offset.xb = self.H[self.idx-1][0]
        self.formation_offset.yb = self.H[self.idx-1][1]

    def update_neightbor_states(self, neighbor_pos, neighbor_vel):
        """
        @Description :更新邻居节点状态
        @Param:
        @Returns     :
        """
        self._neighbor_pos = neighbor_pos
        self._neighbor_vel = neighbor_vel

    def calculate_formation_output(self):
        """
        @Description :计算编队输出
        @Param:
        @Returns     :
        """
        dt = self._dt
        dt = 0.1 
        A = np.array([[1, 0, dt, 0],
                      [0, 1, 0, dt],
                      [0, 0, 1, 0],
                      [0, 0, 0, 1]])
        B = np.array([[1/2*dt**2, 0],
                      [0, 1/2*dt**2],
                      [dt, 0],
                      [0, dt]])

        B_inv = np.linalg.pinv(B)
        
        # self.K = [4.5465, 5.0455] # dt = 0.02
        # self.K = np.array([2, 2.039]) # dt = 0.05
        # self.K = np.array([[2, 2.039], [2, 2.039]]) # dt = 0.05
        self.K = np.array([[0.8, 1.0316],[0.6318, 1.0316]]) # dt = 0.1
        # self.K = np.array([0.6318, 1.0316]) # dt = 0.1
        
        
        # self.Kpx = -10
        # self.Kvx = -8
        # self.Kpy = -5
        # self.Kvy = -8
        # self.gamma = 0.5

        # pos_error = np.zeros((self.agent_num, 2))
        # vel_error = np.zeros((self.agent_num, 2))
        pos_error_sum = np.zeros((1,2))
        vel_error_sum =np.zeros((1,2))

        rot_matrix = np.array([[self._led_cos_dir, self._led_sin_dir],
                               [-self._led_sin_dir, self._led_cos_dir]])
        # print("rot_matrix:",rot_matrix)
        self.H_rot = self.H @ np.kron(np.eye(2), rot_matrix)
        
        self.H_rot[:,2] = self.H[:,1] * self._led_yaw_dot
        self.H_rot[:,3] = -self.H[:,0] * self._led_yaw_dot
        # print("H_rot", self.H_rot)
        print("yaw_dot", self._led_yaw_dot)

        v = B_inv @ self.H_rot.T - B_inv @ A @ self.H_rot_prev.T
        v = v.T
        self.H_rot_prev = self.H_rot

        for i in range(self.agent_num):
            pos_error_sum += self.Adj[self.idx-1][i] * (
                (self._neighbor_pos[i,:] - self._neighbor_pos[self.idx-1,:])
                - (self.H_rot[i,0:2] - self.H_rot[self.idx-1,0:2])
            )
            vel_error_sum += self.Adj[self.idx-1][i] * (
                (self._neighbor_vel[i,:] - self._neighbor_vel[self.idx-1,:])
                - (self.H_rot[i,2:4] - self.H_rot[self.idx-1,2:4])
            )

        consensus_error = np.concatenate((pos_error_sum, vel_error_sum), axis=0)
            
        traj_pos_error = np.array([[self._leader_states_f.ned_pos_n - (self._fw_state_f.ned_pos_n - self.H_rot[self.idx-1, 0]), self._leader_states_f.ned_pos_e - (self._fw_state_f.ned_pos_e - self.H_rot[self.idx-1, 1])]])
        traj_vel_error = np.array([[self._leader_states_f.ned_vel_n - (self._fw_state_f.ned_vel_n - self.H_rot[self.idx-1, 2]), self._leader_states_f.ned_vel_e - (self._fw_state_f.ned_vel_e - self.H_rot[self.idx-1, 3])]])
        traj_error = np.concatenate((traj_pos_error, traj_vel_error), axis=0)
        
        # u = self.K @ (consensus_error + traj_error) + v[self.idx-1,:]
        # u = self.K @ (consensus_error + traj_error)
        u = self.K @ traj_error + v[self.idx-1,:]
        # u = self.K @ traj_error
        
        self.ux = u[0][0]
        self.uy = u[1][1]
        print("u", u)
        print("v", v)

        # pos_error_sum /= self.agent_num
        # vel_error_sum /= self.agent_num

        # print("idx:, vel_error_sum: ", self.idx, vel_error_sum)
        # print("self._neighbor_vel ", self._neighbor_vel)

        # pos_error = self._neighbor_pos - self._neighbor_pos[self.idx-1]
        # vel_error = self._neighbor_vel - self._neighbor_vel[self.idx-1]
        # pos_error_sum = np.sum(pos_error, axis=0)
        # vel_error_sum = np.sum(vel_error, axis=0)

        # self.ux = self.Kp * self.fw_error.P_N
        # self.uy = self.Kp * self.fw_error.P_E
        # self.ux = self.Kp * self.fw_error.P_N + self.Kv * (self._leader_states_f.ned_vel_n - self._fw_state_f.ned_vel_n)
        # self.uy = self.Kp * self.fw_error.P_E + self.Kv * (self._leader_states_f.ned_vel_e - self._fw_state_f.ned_vel_e)
        # if self.idx == 1:
        #     self.gamma = 0
        #     self.ux = self.Kpx * (
        #         ((self._fw_state_f.ned_pos_n - self.H_rot[self.idx-1][0]) - self._leader_states_f.ned_pos_n)
        #         + self.gamma * (pos_error_sum[0])
        #     ) + self.Kvx * (
        #         (self._fw_state_f.ned_vel_n - self._leader_states_f.ned_vel_n)
        #         + self.gamma * (vel_error_sum[0])
        #     )
        #     self.uy = self.Kpy * (
        #         ((self._fw_state_f.ned_pos_e - self.H_rot[self.idx-1][1]) - self._leader_states_f.ned_pos_e)
        #         + self.gamma * (pos_error_sum[1])
        #     ) + self.Kvy * (
        #         (self._fw_state_f.ned_vel_e - self._leader_states_f.ned_vel_e)
        #         + self.gamma * (vel_error_sum[1])
        #     )
        # else:
        #     self.gamma = 1
        #     self.ux = self.Kpx * (
        #         self.gamma * (pos_error_sum[0])
        #     ) + self.Kvx * (
        #         + self.gamma * (vel_error_sum[0])
        #     )
        #     self.uy = self.Kpy * (
        #         self.gamma * (pos_error_sum[1])
        #     ) + self.Kvy * (
        #         self.gamma * (vel_error_sum[1])
        #     )

        self.fw_error.pos_error_sum_x = pos_error_sum[0][0]
        self.fw_error.pos_error_sum_y = pos_error_sum[0][1]
        self.fw_error.vel_error_sum_x = vel_error_sum[0][0]
        self.fw_error.vel_error_sum_y = vel_error_sum[0][1]
        self.fw_error.pos_error_x = traj_pos_error[0][0]
        self.fw_error.pos_error_y = traj_pos_error[0][1]
        self.fw_error.vel_error_x = traj_vel_error[0][0]
        self.fw_error.vel_error_y = traj_vel_error[0][1]
        # self.fw_sp.pos_error_x = self._fw_state_f.ned_pos_n - self.H_rot[self.idx-1][0] - self._leader_states_f.ned_pos_n
        # self.fw_sp.pos_error_y = self._fw_state_f.ned_pos_e - self.H_rot[self.idx-1][1] - self._leader_states_f.ned_pos_e
        # self.fw_sp.vel_error_x = self._fw_state_f.ned_vel_n - self.H_rot[self.idx-1][2] - self._leader_states_f.ned_vel_n
        # self.fw_sp.vel_error_y = self._fw_state_f.ned_vel_e - self.H_rot[self.idx-1][3] - self._leader_states_f.ned_vel_e
        # self.fw_sp.pos_error_sum_x = pos_error_sum[0][0] + traj_pos_error[0][0]
        # self.fw_sp.pos_error_sum_y = pos_error_sum[0][1] + traj_pos_error[0][1]
        # self.fw_sp.vel_error_sum_x = vel_error_sum[0][0] + traj_vel_error[0][0]
        # self.fw_sp.vel_error_sum_y = vel_error_sum[0][1] + traj_vel_error[0][1]
        self.fw_error.ux = self.ux
        self.fw_error.uy = self.uy
        # print("PXk: %f; PYk: %f", self.fw_error.PXk, self.fw_error.PYk)

    def fw_states_callback(self, msg: State):
        self.current_state = msg

    def reset_formation_controller(self):
        """
        @Description :重置控制器
        @Param:
        @Returns     :
        """
        self._rest_speed_pid = True
        self._rest_tecs = True

    def set_mix_Xerr_params(self, input_params: S_MixXerrParams):
        """
        @Description :设定编队混合误差产生的参数
        @Param:
        @Returns     :
        """
        self._mix_Xerr_params = input_params

    def set_mix_Yerr_params(self, input_params: S_MixYerrParams):
        """
        @Description :设定滚转角混合误差产生的参数
        @Param:
        @Returns     :
        """
        self._mix_Yerr_params = input_params

    def get_mix_Xerr_params(self):
        """
        @Description :获取编队混合误差产生的参数
        @Param:
        @Returns     :
        """
        return self._mix_Xerr_params

    def get_min_Yerr_params(self):
        """
        @Description :获取滚转角混合误差产生的参数
        @Param:
        @Returns     :
        """
        return self._mix_Yerr_params

    def set_tecs_params(self, input_params: TECSParams):
        """
        @Description :设定tecs控制参数
        @Param:
        @Returns     :
        """
        self._tecs_params = input_params

    def filter_led_fol_states(self):
        self._fw_state_f = self.fw_state
        self._leader_states_f = self.leader_states

        if self._use_the_filter:
            self._leader_states_f.global_vel_n = self._led_glo_vel_x_filter.update(
                self._leader_states_f.global_vel_n
            )
            self._leader_states_f.global_vel_e = self._led_glo_vel_y_filter.update(
                self._leader_states_f.global_vel_e
            )

    def get_plane_to_vector(self, origin: Point, target: Point):
        """
        @Description :获取飞机当前位置到期望位置的指向向量,这里的target和origin均采用的是ned坐标
        @Param:
        @Returns     :
        """
        out = Point(target.x - origin.x, target.y - origin.y)
        return out

    def control_formation(self):
        """
        @Description :控制器主函数
        @Param:
        @Returns     :
        """
        ros_time = rospy.Time.now()
        now = ros_time.to_sec()
        self._dt = constrain(
            now - self.ctrl_timestamp, self._dt_min, self._dt_max
        )
        self.ctrl_timestamp = now
        # print("self._dt",self._dt)
        # self._dt = self._dt_max
        # self._dt = 0.05
        # self._dt = 0.1

        """
        0. 原始数据滤波，飞机的状态保存在滤波后的状态值里
        """

        self.filter_led_fol_states()

        if not self.identify_led_fol_states():
            rospy.loginfo("警告：领机或从机未在飞行中")
            return

        """
        1. 根据队形要求，计算出从机期望位置的在领机航迹坐标系下的位置，然后再到->GPS位置（期望经纬高）；
        在此之中注意领机航迹方向的选择。
          a. 计算领机机头方向
          b. 计算从机期望位置的gps位置
        """
        led_airspd_x = (
            self._leader_states_f.wind_estimate_n + self._leader_states_f.global_vel_n
        )
        led_airspd_y = (
            self._leader_states_f.wind_estimate_e + self._leader_states_f.global_vel_e
        )

        self._led_airsp.set_vec_ele(led_airspd_x, led_airspd_y)
        self._led_gspeed_2d.set_vec_ele(
            self._leader_states_f.global_vel_n, self._leader_states_f.global_vel_e
        )

        # 根据计算空速与读取空速的判断空速状态是否正确
        if self._led_airsp.len() - self._leader_states_f.air_speed >= 3.0:
            self._led_airspd_states_vaild = False
        else: 
            self._led_airspd_states_vaild = True

        # 判断本机地速太小以及航向角是否已知
        if self._led_gspeed_2d.len() <= 3.0:
            if self._leader_states_f.yaw_valid:
                self._led_cos_dir = math.cos(self._leader_states_f.yaw_angle)
                self._led_sin_dir = math.sin(self._leader_states_f.yaw_angle)
                rospy.loginfo("领机地速太小，选用领机航向角")
            else:
                self._led_cos_dir = 0.0
                self._led_sin_dir = 0.0
                rospy.loginfo("领机地速太小，领机航向角未知")
        else:
            # 领机地速正常
            self._led_cos_dir = self._led_gspeed_2d.x / self._led_gspeed_2d.len()
            self._led_sin_dir = self._led_gspeed_2d.y / self._led_gspeed_2d.len()
            rospy.loginfo("领机地速正常，选用领机地速方向")

        # 保证归一化的结果，求领机的速度方向
        led_dir_unit = Vector(self._led_cos_dir, self._led_sin_dir)
        led_dir_unit.normlized()

        # 计算领机偏航角和偏航角速度，并滤波
        led_yaw_ = np.arctan2(self._led_sin_dir, self._led_cos_dir)
        # print("led_yaw_:",led_yaw_)
        # self._led_yaw = self._led_yaw_filter.update(led_yaw_)
        self._led_yaw = led_yaw_
        delta_yaw_ = self._led_yaw - self._led_yaw_prev
        delta_yaw_ = (delta_yaw_ + np.pi) % (2 * np.pi) - np.pi
        led_yaw_dot_ = delta_yaw_/ self._dt
        self._led_yaw_dot = self._led_yaw_dot_filter.update(led_yaw_dot_)
        self._led_yaw_prev = self._led_yaw
        
        self.fw_error.led_yaw_rate = self._led_yaw_dot

        # 计算编队偏差下，从机相对于领机的ned偏差
        if self.idx == 1:
            self.formation_offset.zb = 0
        elif self.idx == 2:
            self.formation_offset.zb = 10
        elif self.idx == 3:
            self.formation_offset.zb = -10
        self.formation_offset.ned_n = (
            self.formation_offset.xb * self._led_cos_dir
            + self.formation_offset.yb * self._led_sin_dir
        )
        self.formation_offset.ned_e = (
            - self.formation_offset.xb * self._led_sin_dir
            + self.formation_offset.yb * self._led_cos_dir
        )
        # print("formation_offset_n:", self.formation_offset.ned_n)
        # print("formation_offset_e:", self.formation_offset.ned_e)
        self.formation_offset.ned_d = self.formation_offset.zb

        # TODO 将ned坐标转化为经纬度坐标

        self.fw_sp.ned_pos_n = (
            self.leader_states.ned_pos_n + self.formation_offset.ned_n
        )
        self.fw_sp.ned_pos_e = (
            self.leader_states.ned_pos_e + self.formation_offset.ned_e
        )
        self.fw_sp.ned_pos_d = (
            self.leader_states.ned_pos_d + self.formation_offset.ned_d
        )
        self.fw_sp.altitude = -self.fw_sp.ned_pos_d

        """
        2. 计算从机期望位置在自身ned坐标下的位置：
          1) 本机地速方向
          2) 本机航迹角
          3) 有误
        """

        fw_airspd_x = self._fw_state_f.wind_estimate_x + self._fw_state_f.global_vel_n
        fw_airspd_y = self._fw_state_f.wind_estimate_y + self._fw_state_f.global_vel_e

        self._fw_airspd.set_vec_ele(fw_airspd_x, fw_airspd_y)
        self._fw_gspeed_2d.set_vec_ele(
            self._fw_state_f.global_vel_n, self._fw_state_f.global_vel_e
        )

        # 根据计算空速与读取空速的判断空速状态是否正确
        if self._fw_gspeed_2d.len() - self.fw_state.air_speed >= 3.0:
            self._fw_airspd_states_vaild = False
        else: 
            self._fw_airspd_states_vaild = True
        # 判断本机地速太小以及航向角是否已知
        if self._fw_gspeed_2d.len() <= 3.0:
            if self._fw_state_f.yaw_valid:
                self._fw_cos_dir = math.cos(self._fw_state_f.yaw_angle)
                self._fw_sin_dir = math.sin(self._fw_state_f.yaw_angle)
                rospy.loginfo("从机地速太小，选用从机航向角")
            else:
                self._fw_cos_dir = 0.0
                self._fw_sin_dir = 0.0
                rospy.loginfo("从机地速太小，且从机航向角未知")
        else:
            # 选用从机地速方向
            self._fw_cos_dir = self._fw_gspeed_2d.x / self._fw_gspeed_2d.len()
            self._fw_sin_dir = self._fw_gspeed_2d.y / self._fw_gspeed_2d.len()

        # 对本机速度进行归一化，求本机速度方向单位向量
        fw_dir_unit = Vector(self._fw_cos_dir, self._fw_sin_dir)
        fw_dir_unit.normlized()

        """
        3. 计算从机期望位置与当前位置的误差在从机航迹系下的投影
        """

        # 用经纬度坐标来表示期望位置，当前是用的是ned坐标
        pos_sp = Point(self.fw_sp.ned_pos_n, self.fw_sp.ned_pos_e)  # 期望位置
        current_pos = Point(
            self.fw_state.ned_pos_n, self.fw_state.ned_pos_e
        )  # 当前位置
        vec_plane_sp = Vector(
            pos_sp.x - current_pos.x, pos_sp.y - current_pos.y
        )  # 飞机到期望点的向量（误差向量）

        self.fw_error.P_N = vec_plane_sp.x
        self.fw_error.P_E = vec_plane_sp.y
        self.fw_error.P_D = (self.fw_sp.altitude - self.fw_state.altitude)
        self.fw_error.P_NE = math.sqrt((self.fw_error.P_N**2 + self.fw_error.P_E**2))

        self.fw_error.PXk = led_dir_unit.dot(vec_plane_sp)
        directioon = led_dir_unit.cross(vec_plane_sp)
        error_vert = vec_plane_sp.sub(led_dir_unit.mul(led_dir_unit.dot(vec_plane_sp)))
        if directioon < 0:
            self.fw_error.PYk = - error_vert.len()
        else:
            self.fw_error.PYk = error_vert.len()

        # self.fw_error.PXk = fw_dir_unit.dot(vec_plane_sp)
        # self.fw_error.PYk = fw_dir_unit.cross(vec_plane_sp)

        """
        4. 计算领机从机地速“差”在从机航迹坐标系之中的投影，控制量是地速，所以是地速之差
        """
        led_fol_vel_error = self._led_gspeed_2d.sub(self._fw_gspeed_2d)
        self.fw_error.led_fol_vxk = fw_dir_unit.dot(led_fol_vel_error)
        self.fw_error.led_fol_vyk = fw_dir_unit.cross(led_fol_vel_error)
        self.fw_error.led_fol_vzk = (
            self._leader_states_f.global_vel_d - self._fw_state_f.global_vel_d
        )

        self.fw_error.led_fol_vk = math.sqrt(
            self.fw_error.led_fol_vxk**2
            + self.fw_error.led_fol_vyk**2
            + self.fw_error.led_fol_vzk**2
        )

        """
        5. 计算领机从机速度角度，机体前向右偏为正,范围(0~pi/2),左偏为负，范围（-pi/2~0)
        """
        # 计算领机从机的速度夹角
        led_gsp_Xk = fw_dir_unit.dot(self._led_gspeed_2d)
        led_gsp_Yk = fw_dir_unit.cross(self._led_gspeed_2d)

        if led_gsp_Xk > 0 and led_gsp_Yk > 0: # 右前方
            self.fw_error.led_fol_eta = math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk < 0 and led_gsp_Yk > 0: # 右后方
            self.fw_error.led_fol_eta = math.pi + math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk > 0 and led_gsp_Yk < 0: # 左前方
            self.fw_error.led_fol_eta = math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk < 0 and led_gsp_Yk < 0: # 左后方
            self.fw_error.led_fol_eta = - math.pi + math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk == 0 and led_gsp_Yk < 0: # 正左方
            self.fw_error.led_fol_eta = - math.pi / 2
        elif led_gsp_Xk == 0 and led_gsp_Yk > 0: # 正右方
            self.fw_error.led_fol_eta = math.pi / 2
        elif led_gsp_Yk == 0 and led_gsp_Xk < 0: # 正后方
            self.fw_error.led_fol_eta = - math.pi
        elif led_gsp_Yk == 0 and led_gsp_Xk > 0: # 正前方
            self.fw_error.led_fol_eta = 0
        else: # 速度方向有误
            self.fw_error.led_fol_eta = 0
            rospy.loginfo("领机从机速度夹角有误")

        """
        6. 判断控制分段
        """

        if self.fw_error.PXk > 60.0:
            self.__controller_method = LONG_DIS
            rospy.loginfo("控制分段：远距离")
        else:
            self.__controller_method = CLOSE_DIS
            rospy.loginfo("控制分段：近距离")

        """
        6.5 根据编队控制协议，生成编队控制量
        """
        self.calculate_formation_output()
        # 根据非线性反馈反馈线性化，有
        # a_t = (u_n * cos(psi) + u_e * sin(psi)) * cos(theta) + g * sin(theta)
        # phi = arctan(-u_n * sin(psi) + u_e * cos(psi) / g) + tan(psi) * sin(theta)

        # a_t_sp = (self.ux * self._fw_cos_dir + self.uy * self._fw_sin_dir) / math.cos(self._fw_state_f.pitch_angle) + GRAVITY * math.sin(self._fw_state_f.pitch_angle) # 加速度大小
        # phi_sp = math.atan((-self.ux * self._fw_sin_dir + self.uy * self._fw_cos_dir) / GRAVITY + math.tan(self._fw_state_f.yaw_angle)*math.sin(self._fw_state_f.pitch_angle)) # 横滚角

        a_t_sp = (self.ux * self._fw_cos_dir + self.uy * self._fw_sin_dir) * math.cos(self._fw_state_f.pitch_angle) + GRAVITY * math.sin(self._fw_state_f.pitch_angle) # 加速度大小
        phi_sp = math.atan((-self.ux * self._fw_sin_dir + self.uy * self._fw_cos_dir) / GRAVITY ) # 横滚角
        
        # a_t_sp = self._a_t_sp_filter.update(a_t_sp)
        # phi_sp = self._phi_sp_filter.update(phi_sp)
        
        self.fw_sp.a_t = a_t_sp
        self.fw_sp.phi = phi_sp
        
        # a_t_sp = constrain(a_t_sp, -self.fw_params.maxdec_acc, self.fw_params.maxinc_acc)

        # print("ux: %f, uy: %f" % (self.ux, self.uy))
        # print("a_t_sp: %f, phi_sp: %f" % (a_t_sp, phi_sp))

        """
        7. 根据前向位置误差，产生期望速度大小
        """

        if self.__controller_method == LONG_DIS:
            self.fw_sp.air_speed = self.fw_params.max_airspeed_sp
            self._gspeed_sp_pid.set_prev_output()
        elif self.__controller_method == CLOSE_DIS:
            # 1.产生前向混合误差
            # self.mix_err_Xk = (
            #     self._mix_Xerr_params.kp_p * self.fw_error.PXk
            #     + self._mix_Xerr_params.kv_p * self.fw_error.led_fol_vk
            # )
            # 2.产生期望地速大小
            # if self._rest_speed_pid:
            #     self._rest_speed_pid = False
            #     self._gspeed_sp_pid.reset()

            # 增量式PID
            # self._gspeed_sp_pid.increment_pid(
            #     self.mix_err_Xk,
            #     self._mix_Xerr_params.mix_kp,
            #     self._mix_Xerr_params.mix_ki,
            #     self._mix_Xerr_params.mix_kd,
            # )
            # self.fw_sp.ground_speed = self._gspeed_sp_pid.get_full_output()

            # PID
            # self._gspeed_sp_pid.set_pid_params(0.5, 0.00001, 0.001, 25)
            # self._gspeed_sp_pid.pid_calculate(
            #     0,
            #     - self.mix_err_Xk,
            #     self._dt
            # )
            # self.fw_sp.ground_speed = self._gspeed_sp_pid.get_output()
            # rospy.loginfo("纵向混合误差: {}".format(self.fw_sp.ground_speed))

            # rospy.loginfo("期望地速大小: {}".format(self.fw_sp.ground_speed))

            ###########################################################################################
            # 根据编队控制量，产生期望地速
            # self.fw_sp.ground_speed = self._fw_state_f.ground_speed + a_t_sp * self._dt
            self.fw_sp.ground_speed = self._fw_state_f.ground_speed + a_t_sp
            # print("gspeed_2d:",self._fw_gspeed_2d.len())
            # print("ground_speed_sp",self.fw_sp.ground_speed)
            ###########################################################################################

            # 3. 根据期望地速产生期望空速
            self._fw_wind_vector.set_vec_ele(
                self.fw_state.wind_estimate_x, self.fw_state.wind_estimate_y
            )

            wind_Xk = fw_dir_unit.dot(self._fw_wind_vector)  # 沿着飞机机体前向的风估计

            # self._airspeed_sp = self.fw_sp.ground_speed + wind_Xk
            self._airspeed_sp = self.fw_sp.ground_speed - wind_Xk

            # rospy.loginfo("原始空速设定值: {}".format(self._airspeed_sp))

            # 符合飞机本身的加减速特性
            # ATTENTION:慎用，加上之后,需要加大飞机的前向后相加速度，由于延时作用太强，可能导致不稳定。
            if (
                self._airspeed_sp - self._airspeed_sp_prev
            ) > self.fw_params.maxinc_acc * self._dt:
                self._airspeed_sp = (
                    self._airspeed_sp_prev + self.fw_params.maxinc_acc * self._dt
                )
            elif (
                self._airspeed_sp - self._airspeed_sp_prev
            ) < -self.fw_params.maxinc_acc * self._dt:
                self._airspeed_sp = (
                    self._airspeed_sp_prev - self.fw_params.maxinc_acc * self._dt
                )

            self._airspeed_sp = self._airspeed_sp_filter.update(
                self._airspeed_sp
            )  # 一阶低通滤波

            self._airspeed_sp_prev = self._airspeed_sp

            self.fw_sp.air_speed = constrain(
                self._airspeed_sp,
                self.fw_params.min_airspeed_sp,
                self.fw_params.max_airspeed_sp,
            )
            # rospy.loginfo("最终空速设定值: {}".format(self._airspeed_sp))
        else:
            # 分段有误
            pass

        """
        8. 调用TECS获得期望俯仰角和期望油门
        """

        if self._rest_tecs:
            self._rest_tecs = False
            self._tecs.reset_states()

        # 设置参数，真实的飞机参数还需要实际调参
        self._tecs.set_speed_weight(self._tecs_params.speed_weight)
        self._tecs.set_time_const_throttle(
            self._tecs_params.time_const_throttle
        )  # 这个值影响到总能量——>油门，相当于Kp的倒数
        self._tecs.set_time_const(
            self._tecs_params.time_const
        )  # 这个值影响到能量分配——>俯仰角，相当于Kp的倒数

        self._tecs.enable_airspeed(True)

        if self.fw_sp.altitude - self.fw_state.altitude >= 10:  # 判断一下是否要进入爬升
            self._tecs_params.climboutdem = True
        else:
            self._tecs_params.climboutdem = False

        self._tecs.update_vehicle_state_estimates(
            self._fw_state_f.air_speed,
            self._fw_state_f.rotmat,
            self._fw_state_f.body_acc,
            self._fw_state_f.altitude_lock,
            self._fw_state_f.in_air,
            self._fw_state_f.altitude,
            self._vz_valid,
            self._fw_state_f.ned_vel_d,
            self._fw_state_f.body_acc[2],
        )

        self._tecs.update_pitch_throttle(
            self.fw_state.rotmat,
            self._fw_state_f.pitch_angle,
            self._fw_state_f.altitude,
            self.fw_sp.altitude,
            self.fw_sp.air_speed,
            self._fw_state_f.air_speed,
            self._tecs_params.EAS2TAS,
            self._tecs_params.climboutdem,
            self._tecs_params.climbout_pitch_min_rad,
            self.fw_params.throttle_min,
            self.fw_params.throttle_max,
            self.fw_params.cruise_throttle,
            self.fw_params.pitch_min_rad,
            self.fw_params.pitch_max_rad,
        )

        self.fw_cmd.pitch = self._tecs.get_pitch_setpoint()
        self.fw_cmd.throttle = self._tecs.get_throttle_setpoint()
        # print("TECS mode:", self._tecs.tecs_mode())

        """
        9. 调用L1控制器获得期望滚转角
        """

        if self.__controller_method == LONG_DIS:
            self._lateral_mode = 0
            # if True: # 目前只考虑用L1控制器
            # L1 控制方法
            self._l1_controller.l1_controller(
                current_pos,
                pos_sp,
                self._fw_gspeed_2d,
                self._fw_state_f.air_speed,
            )

            roll_cmd = self._l1_controller.nav_roll()
        elif self.__controller_method == CLOSE_DIS:
            # elif False:
            # 编队位置与角度误差控制方法
            # if self.fw_error.led_fol_eta <= deg_2_rad(20.0) and self.fw_error.led_fol_eta >= deg_2_rad(-20.0):
            # self._lateral_mode = 1
            # rospy.loginfo("横向控制：混合误差")
            # self._mix_Yerr_params.keta_p = 2
            # self._mix_Yerr_params.kp_p = 0.08

            # self.mix_err_Yk = self._mix_Yerr_params.keta_p * self.fw_error.led_fol_eta + self._mix_Yerr_params.kp_p * self.fw_error.PYk
            # rospy.loginfo("fw_error.PYk:%f",self.fw_error.PYk)
            # rospy.loginfo("led_fol_eta:%f",self.fw_error.led_fol_eta)
            # rospy.loginfo("横向混合误差：%f", self.mix_err_Yk)

            # if self._reset_lateral_controller:
            #     self._reset_lateral_controller = False
            #     self._roll_sp_pid.reset()

            # 增量式PID
            # self._roll_sp_pid.increment_pid(self.mix_err_Yk, self._mix_Yerr_params.mix_kp, self._mix_Yerr_params.mix_ki, self._mix_Yerr_params.mix_kd)
            # phi_dot_sp = self._roll_sp_pid.get_full_output()

            # PID
            # self._roll_sp_pid.set_pid_params(self._mix_Yerr_params.mix_kp, self._mix_Yerr_params.mix_ki, self._mix_Yerr_params.mix_kd, deg_2_rad(30))
            # self._roll_sp_pid.pid_calculate(0, - self.mix_err_Yk, self._dt)
            # phi_dot_sp = self._roll_sp_pid.get_output()

            # roll_cmd = math.atan(phi_dot_sp * self._fw_gspeed_2d.len() / GRAVITY)
            # rospy.loginfo("phi_dot_sp:%f", phi_dot_sp)
            # rospy.loginfo("roll_cmd:%f", roll_cmd)

            # self._reset_eta_2_roll_sp_pid = True

            ##########################################################################################################################################
            # 根据编队控制量生成期望滚转角
            roll_cmd = phi_sp
            ##########################################################################################################################################

        # else:
        #     self._lateral_mode = 2
        #     rospy.loginfo("横向控制：速度角误差")
        #     if self._reset_eta_2_roll_sp_pid:
        #         self._reset_eta_2_roll_sp_pid = False
        #         self._eta_2_roll_sp_pid.reset()

        #     self._eta_2_roll_sp_pid.set_pid_params(1,0,0,deg_2_rad(30))
        #     self._eta_2_roll_sp_pid.pid_calculate(0, -self.fw_error.led_fol_eta, self._dt)
        #     roll_cmd = self._eta_2_roll_sp_pid.get_output()

        #     self._reset_lateral_controller = True

        else:
            # 分段有误，无法计算期望滚转
            rospy.loginfo("分段有误，无法计算期望滚转角")
            pass

        roll_cmd = constrain(
            roll_cmd, -self.fw_params.roll_max_rad, self.fw_params.roll_max_rad
        )
        self.fw_cmd.roll = roll_cmd
        
        # rospy.loginfo(
        #     "roll_cmd: %f, pitch_cmd: %f, yaw_cmd: %f, throttle_cmd: %f",
        #     self.fw_cmd.roll,
        #     self.fw_cmd.pitch,
        #     self.fw_cmd.yaw,
        #     self.fw_cmd.throttle,
        # )

        # print("PXk: {}".format(self.fw_error.PXk))
        # print("PYk: {}".format(self.fw_error.PYk))
        # print("led_fol_vk:", self.fw_error.led_fol_vk)
        # print("led_fol_eta:", self.fw_error.led_fol_eta)
        # print("mix_err_Xk: {}".format(self.mix_err_Xk))
        # print("mix_err_Yk: {}".format(self.mix_err_Yk))

        # 将当前变量的值创建为一个新的 DataFrame
        # data = pd.DataFrame(
        #     {
        #         "fw_pos_x": [self.fw_state.ned_pos_n],
        #         "fw_pos_y": [self.fw_state.ned_pos_e],
        #         "led_pos_x": [self.leader_states.ned_pos_n],
        #         "led_pos_y": [self.leader_states.ned_pos_e],
        #         "PXk": [self.fw_error.PXk],
        #         "PYk": [self.fw_error.PYk],
        #         "led_fol_vk": [self.fw_error.led_fol_vk],
        #         "led_fol_eta": [self.fw_error.led_fol_eta],
        #         "mix_err_Xk": [self.mix_err_Xk],
        #         "mix_err_Yk": [self.mix_err_Yk],
        #         "ground_speed_sp": [self.fw_sp.ground_speed],
        #         "air_speed_sp": [self.fw_sp.air_speed],
        #         "roll_sp": [self.fw_cmd.roll],
        #         "fw_sp.altitude": [self.fw_sp.altitude],
        #         "fw_state.altitude": [self.fw_state.altitude]
        #     }
        # )

        # csv_filename = 'output1.csv'
        # if not self.data_head:
        #     # 如果是第一次写入，写入表头
        #     data.to_csv(csv_filename, index=False, header=True)
        #     self.data_head = True
        # else:
        #     # 如果不是第一次写入，不写入表头
        #     data.to_csv(csv_filename, index=False, mode='a', header=False)
